#include "Octree.h"
#include <stdexcept>

namespace SEngine
{
    OctreeNode & Octree::CreateNode(BoundingBox boundingBox)
    {
        auto node = std::make_shared<OctreeNode>(*this, boundingBox);
        m_nodes.emplace_back(node);
        return *node;
    }

    void Octree::DestroyNode(OctreeNode& node)
    {
        m_nodes.erase(std::remove_if(m_nodes.begin(), m_nodes.end(), [&node](std::shared_ptr<OctreeNode> elem) { return elem.get() == &node; }), m_nodes.end());
    }

    OctreeNode & Octree::Root()
    {
        if (m_nodes.empty()) {
            throw std::runtime_error("Octree empty!");
        }
        return **m_nodes.begin();
    }

    OctreeNode& Octree::Sample(glm::vec3 pos)
    {
        if (m_nodes.empty()) {
            throw std::runtime_error("Octree empty!");
        }
        auto & root = **m_nodes.begin();
        if (!root.GetBoundingBox().Contained(pos)) {
            throw std::runtime_error("Octree Sample error!");   
        }
        std::function<OctreeNode&(OctreeNode&)> SampleImpl = [pos, &SampleImpl](OctreeNode& node) {
            if (node.IsLeaf()) {
                return node;
            }
            std::shared_ptr<OctreeNode> nextNode{nullptr};
            node.ForeachChildren([&nextNode, pos](OctreeNode & subNode) {
                if (subNode.GetBoundingBox().Contained(pos)) {
                    nextNode = subNode.shared_from_this();
                }
            });
            return SampleImpl(*nextNode);
        };
        return SampleImpl(root);
    }

} // namespace SEngine
